#include "WalkInstructionHandler.h"


WalkInstructionHandler::WalkInstructionHandler( AiboState* aiboState )
{
	this->setAiboState( aiboState );
	m_walkInstructionList = new QVector<WalkInstruction>();
	this->getInstructionList()->clear();
}

WalkInstructionHandler::~WalkInstructionHandler(void)
{
}

void WalkInstructionHandler::updateAiboState() {
	if( !this->getInstructionList()->isEmpty() ) {
		qDebug() << "WalkInstructions: Updating AiboState - Instructions left: " << this->getInstructionList()->size();
		WalkInstruction instruction = this->getInstructionList()->back();
		this->getInstructionList()->pop_back();
		this->sendGeneralInstruction( &instruction );
	}
}

bool WalkInstructionHandler::addInstruction( WalkInstruction wInstruction ) {
	bool success = true;
	if( !this->getInstructionList()->isEmpty() ) {
		if( this->getInstructionList()->front().getType() != wInstruction.getType() ) {
			this->getInstructionList()->push_front( wInstruction );
		}
		else if (this->getInstructionList()->front().getValue() != wInstruction.getValue() ) {
				this->getInstructionList()->push_front( wInstruction );
		}
		else
			success = false;
	}
	else {
		this->getInstructionList()->push_front( wInstruction );
	}
	return success;
}

void WalkInstructionHandler::resetAiboState() 
{
	qDebug() << "WalkInstructions: Reseting AiboState";
	addInstruction( WalkForwardInstruction( 0.0f ) );
	addInstruction( WalkRotateInstruction( 0.0f ) );
	addInstruction( WalkStrafeInstruction( 0.0f ) );
}

void WalkInstructionHandler::sendGeneralInstruction( WalkInstruction* instruction )
{
	switch( instruction->getType() ) {
	case CMD_WALK_FORWARD:
		sendInstruction( (WalkForwardInstruction*)instruction );
		break;
	case CMD_WALK_ROTATE:
		sendInstruction( (WalkRotateInstruction*)instruction );
		break;
	case CMD_WALK_STRAFE:
		sendInstruction( (WalkStrafeInstruction*)instruction );
		break;
	}
}

bool WalkInstructionHandler::sendInstruction( WalkForwardInstruction* forwardInstruction ) {
	bool success = false;
	if( this->getAiboState() != NULL && forwardInstruction != NULL ) {
		if( forwardInstruction->isOffInstruction() && this->isForwardOn() ) {
			if( !this->getAiboState()->isWalkInstructionOn() || this->getAiboState()->isForward() ) {
				qDebug() << "WalkInstructions: Forward Off";
				this->turnForwardOff();
				success = true;
			}
		}
		else {
			if( !this->getAiboState()->isWalkInstructionOn() ) {
				qDebug() << "WalkInstructions: Forward On - No Previous Instructions";
				this->turnForwardOn();
				success = true;
			}
			else if( this->getAiboState()->isForward() && this->getAiboState()->getForwardSpeed() != forwardInstruction->getValue() ) {
				qDebug() << "WalkInstructions: Forward On - Previous Speed Changed";
				this->turnForwardOn();
				success = true;
			}
			else {
				this->addInstruction( *forwardInstruction );
			}
		}
		if( success )
			this->getAiboState()->startForward( forwardInstruction->getValue() );
		else
			qDebug() << "WalkInstructions: No Forward Msg Sent";
	}
	return success;
}

bool WalkInstructionHandler::sendInstruction( WalkRotateInstruction* rotateInstruction ) {
	bool success = false;
	if( this->getAiboState() != NULL ) {
		if( rotateInstruction->isOffInstruction() && this->isRotateOn() ) {
			if( !this->getAiboState()->isWalkInstructionOn() || this->getAiboState()->isTurning() ) {
				qDebug() << "WalkInstructions: Turning Off";
				this->turnRotateOff();
				success = true;
			}
		}
		else {
			if( !this->getAiboState()->isWalkInstructionOn() ) {
				qDebug() << "WalkInstructions: Turning On - No Previous Instructions";
				this->turnRotateOn();
				success = true;
			}
			else if( this->getAiboState()->isTurning() && this->getAiboState()->getTurnSpeed() != rotateInstruction->getValue() ) {
				qDebug() << "WalkInstructions: Turning On - Previous Speed Changed";
				this->turnRotateOn();
				success = true;
			}
			else {
				this->addInstruction( *rotateInstruction );
			}
		}
		if( success )
			this->getAiboState()->startTurning( rotateInstruction->getValue() );
		else
			qDebug() << "WalkInstructions: No Turning Msg Sent";
	}
	return success;
}

bool WalkInstructionHandler::sendInstruction( WalkStrafeInstruction* strafeInstruction ) {
	bool success = false;
	if( this->getAiboState() != NULL ) {
		if( strafeInstruction->isOffInstruction() && this->isStrafeOn() ) {
			if( !this->getAiboState()->isWalkInstructionOn() || this->getAiboState()->isStrafe() ) {
				qDebug() << "WalkInstructions: Strafing Off";
				this->turnStrafeOff();
				success = true;
			}
		}
		else {
			if( !this->getAiboState()->isWalkInstructionOn() ) {
				qDebug() << "WalkInstructions: Strafing On - No Previous Instructions";
				this->turnStrafeOn();
				success = true;
			}
			else if( this->getAiboState()->isStrafe() && this->getAiboState()->getStrafeSpeed() != strafeInstruction->getValue() ) {
				qDebug() << "WalkInstructions: Strafing On - Previous Speed Changed";
				this->turnStrafeOn();
				success = true;
			}
			else {
				this->addInstruction( *strafeInstruction );
			}
		}
		if( success )
			this->getAiboState()->startStrafe( strafeInstruction->getValue() );
		else
			qDebug() << "WalkInstructions: No Strafing Msg Sent";
	}
	return success;
}